#include "camodocal.h"
#include "iostream"

#include "ceres/ceres.h"

namespace Camodo {

class CameraOdomErr
{
private:
  Eigen::Quaterniond m_q1, m_q2;
  Eigen::Vector3d m_t1,m_t2;

public:
  CameraOdomErr(Eigen::Quaterniond q1, Eigen::Vector3d t1,    // odomter
                                    Eigen::Quaterniond q2, Eigen::Vector3d t2)   // cam
    : m_q1(q1),m_q2(q2), m_t1(t1),m_t2(t2)
  {}

  template<typename T>
  bool operator() (const T* const q4x1 , const T* const t3x1, const T* const scale, T* residuals) const
  {
    Eigen::Quaternion<T> qrc( q4x1[0], q4x1[1],q4x1[2],q4x1[3]);
    Eigen::Matrix<T,3,1> trc;
    trc<<t3x1[0],t3x1[1], T(0);

    Eigen::Quaternion<T> q_odo = m_q1.cast<T>();
    Eigen::Matrix<T,3,1>  t_odo =  m_t1.cast<T>();
    Eigen::Quaternion<T> q_cc = m_q2.cast<T>();
    Eigen::Matrix<T, 3,1> t_cc = m_t2.cast<T>();

    Eigen::Matrix<T,3,3> R_odo = q_odo.toRotationMatrix();
    Eigen::Matrix<T,3,3> Rrc = qrc.toRotationMatrix();

    Eigen::Matrix<T, 3,1> t_err = (R_odo - Eigen::Matrix<T,3,3>::Identity() ) * trc - (scale[0] * Rrc * t_cc) + t_odo;

    //  q is unit quaternion,   q.inv() = q.conjugate();
    //  q_odo * q_oc = q_oc * q_cc  -->   q_oc.conjugate() * q_odo * q_oc * q_cc.conjugate() = 0;
    Eigen::Quaternion<T> q_err = qrc.conjugate() * q_odo * qrc * q_cc.conjugate();
    Eigen::Matrix<T,3,3> R_err = q_err.toRotationMatrix();

    T roll, pitch, yaw;
    mat2RPY(R_err,roll, pitch,yaw);

    residuals[0] = t_err[0];
    residuals[1] = t_err[1];
    residuals[2] = t_err[2];
    residuals[3] = roll;
    residuals[4] = pitch;
    residuals[5] = yaw;

    return true;
  }

};

//#define LOSSFUNCTION
void CamOdoCalibration::refineEstimate(Eigen::Matrix4d &Trc, double scale,
                                const std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > &quats_odo,
                                const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &tvecs_odo,
                                const std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > &quats_cam,
                                const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &tvecs_cam)
{
   Eigen::Quaterniond q(Trc.block<3,3>(0,0));
   double q_coeffs[4] = {q.w(),q.x(),q.y(),q.z()};
   double t_coeffs[3] = {Trc(0,3),Trc(1,3),Trc(2,3)};

   ceres::Problem problem;
   for(size_t i = 0; i< quats_odo.size(); ++i)
   {
      ceres::CostFunction * costfunction =
          new ceres::AutoDiffCostFunction<CameraOdomErr, 6,4,2,1>(
            new CameraOdomErr(quats_odo.at(i) , tvecs_odo.at(i), quats_cam.at(i) , tvecs_cam.at(i)));   //  residual : 6 ,  rotation: 4

#ifdef LOSSFUNCTION
      //ceres::LossFunctionWrapper* loss_function(new ceres::HuberLoss(1.0), ceres::TAKE_OWNERSHIP);
      ceres::LossFunction * loss_function = new ceres::HuberLoss(1.0);
      problem.AddResidualBlock(costfunction, loss_function, q_coeffs, t_coeffs, &scale);
#else
      problem.AddResidualBlock(costfunction, NULL, q_coeffs, t_coeffs, &scale);
#endif

   }

   ceres::LocalParameterization* quaternionParameterization = new ceres::QuaternionParameterization;
   problem.SetParameterization(q_coeffs,quaternionParameterization);

   ceres::Solver::Options options;
   options.linear_solver_type = ceres::DENSE_QR;
   options.max_num_iterations = 100;

   ceres::Solver::Summary summary;
   ceres::Solve (options, &problem, & summary);

   q = Eigen::Quaterniond(q_coeffs[0],q_coeffs[1],q_coeffs[2],q_coeffs[3]);

   Trc.block<3,3>(0,0) = q.toRotationMatrix();
   Trc.block<3,1>(0,3) << t_coeffs[0],t_coeffs[1],t_coeffs[2];

}


bool CamOdoCalibration::estimate(const std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > &quats_odo,
                                 const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &tvecs_odo,
                                 const std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > &quats_cam,
                                 const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &tvecs_cam,
                                 Eigen::Matrix4d &Trc)
{

  // estimate R_yx first
  Eigen::Matrix3d R_yx;
  estimateRyx(quats_odo,tvecs_odo,quats_cam,tvecs_cam,R_yx);

  size_t motionCnt = quats_odo.size();

  Eigen::MatrixXd G = Eigen::MatrixXd::Zero(2*motionCnt, 4);
  Eigen::MatrixXd w(2*motionCnt, 1);

  for(unsigned int i=0; i< motionCnt; ++i)
  {
    const Eigen::Quaterniond& q_odo = quats_odo.at(i);
    const Eigen::Vector3d& t_odo = tvecs_odo.at(i);
    const Eigen::Quaterniond& q_cam = quats_cam.at(i);
    const Eigen::Vector3d& t_cam = tvecs_cam.at(i);

    Eigen::Matrix2d J;
    J = q_odo.toRotationMatrix().block<2,2>(0,0) - Eigen::Matrix2d::Identity();

    // project tvec_cam to plane with normal defined by 3rd row of R_yx
    Eigen::Vector3d n;
    n = R_yx.row(2);
    Eigen::Vector3d pi = R_yx *( t_cam - t_cam.dot(n)*n ); //这个投影是不是写错了，感觉没啥用呀，只是把第三维向量置0了
    Eigen::Vector3d pi_d = R_yx * t_cam;

    Eigen::Matrix2d K;
    K << -pi(0),pi(1),-pi(1),-pi(0);

    G.block<2,2>(i * 2, 0) = J;
    G.block<2,2>(i * 2, 2) = K;

    w.block<2,1>(i*2, 0) = -t_odo.block<2,1>(0,0);

  }

  Eigen::Vector4d m;
  m = G.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(w);

  Eigen::Vector2d trc(m(0),m(1));
  double alpha = atan2(m(3),m(2));
  double scale = m.block<2,1>(2,0).norm();     // scale^2 = m(2)^2 + m(3)^2

  Trc.setIdentity();
  Trc.block<3,3>(0,0) = Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitZ() ) * R_yx;
  Trc.block<2,1>(0,3) = trc;

  //if(m_verbose)
  {
      std::cout<<" ##INFO: Before refinement: Trc = : "<<std::endl<<Trc<<std::endl;
      std::cout<<" ##INFO: scale = : "<<scale<<std::endl;
      std::cout<<" ##INFO: Yaw = : "<<alpha<<std::endl;
  }

  refineEstimate(Trc,scale,quats_odo,tvecs_odo,quats_cam,tvecs_cam);

  {
      std::cout<<" ##INFO: after refinement: Trc = : "<<std::endl<<Trc<<std::endl;
      std::cout<<" ##INFO: scale = : "<<scale<<std::endl;
  }

    return true;
}

bool CamOdoCalibration::estimateRyx(const std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > &quats_odo,
                                    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &tvecs_odo,
                                    const std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > &quats_cam,
                                    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &tvecs_cam,
                                    Eigen::Matrix3d &R_yx)
{
      size_t motionCnt = quats_odo.size( );

      Eigen::MatrixXd M(motionCnt*4, 4);
      M.setZero();

      for(size_t i = 0; i < quats_odo.size(); ++i)
      {
          const Eigen::Quaterniond& q_odo = quats_odo.at(i);
          const Eigen::Vector3d& t_odo = tvecs_odo.at(i);
          const Eigen::Quaterniond& q_cam = quats_cam.at(i);
          const Eigen::Vector3d& t_cam = tvecs_cam.at(i);

          M.block<4,4>(i*4, 0) = QuaternionMultMatLeft(q_odo) - QuaternionMultMatRight(q_cam);
      }

      //TODO:: M^T * M
      Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullU |Eigen::ComputeFullV);

      Eigen::Vector4d v1 = svd.matrixV().block<4,1>(0,2);
      Eigen::Vector4d v2 = svd.matrixV().block<4,1>(0,3);

      double lambda[2];      // solution from  ax^2+ bx + c = 0
      if( !SolveConstraintqyx(v1,v2,lambda[0],lambda[1]))
      {
        //std::cout << "# ERROR: Quadratic equation cannot be solved due to negative determinant." << std::endl;
        return false;
      }

      // choose one lambda
      Eigen::Matrix3d R_yxs[2];
      double yaw[2];

      for( int i = 0; i<2;++i)
      {
          double t = lambda[i] * lambda[i] * v1.dot(v1) + 2 * lambda[i] * v1.dot(v2) + v2.dot(v2);

          // solve constraint ||q_yx|| = 1
          double lambda2 = sqrt(1.0/t);
          double lambda1 = lambda[i] * lambda2;

          Eigen::Quaterniond q_yx;
          q_yx.coeffs() = lambda1 * v1 + lambda2 * v2;

          R_yxs[i] = q_yx.toRotationMatrix();
          double roll,pitch;
          mat2RPY(R_yxs[i], roll, pitch, yaw[i]);
          std::cout<<"roll: "<<roll<<" pitch: "<<pitch<<" yaw: "<<yaw[i]<<std::endl;
      }

      // q_yx  means yaw is zero. we choose the smaller yaw
      if(fabs(yaw[0]) < fabs(yaw[1]) )
      {
          R_yx = R_yxs[0];
      }else
      {
        R_yx = R_yxs[1];
      }


      return true;

}

/*
 *    constraint for q_yx:
 *                         xy = -zw
 *     this can transfrom to a  equation  ax^2+ bx + c = 0
 */
bool CamOdoCalibration::SolveConstraintqyx(const Eigen::Vector4d t1, const Eigen::Vector4d t2, double& x1, double& x2)
{
    double a = t1(0) * t1(1)+ t1(2)*t1(3);
    double b = t1(0) * t2(1)+ t1(1)*t2(0)+t1(2) * t2(3)+ t1(3)*t2(2);
    double c =  t2(0) * t2(1)+ t2(2)*t2(3);

    if ( std::fabs(a) < 1e-10)
    {
        x1 = x2 = -c/b;
        return true;
    }
    double delta2 = b*b - 4.0 * a * c;

    if(delta2 < 0.0) return false;

    double delta = sqrt(delta2);

    x1 = (-b + delta)/(2.0 * a);
    x2 = (-b - delta)/(2.0 * a);

    return true;

}





}
